import machine
import time

# Setup I2C communication (GPIO 4 for SDA, GPIO 5 for SCL on Raspberry Pi Pico)
i2c = machine.I2C(0, sda=machine.Pin(4), scl=machine.Pin(5))  # Adjust GPIO pins as needed

# VEML6040 I2C address and registers
VEML6040_ADDR = 0x10  # Default I2C address of the VEML6040 (check datasheet for your sensor's address)
REG_CONFIG = 0x00  # Configuration register
REG_RED = 0x08     # Register for Red data
REG_GREEN = 0x09   # Register for Green data
REG_BLUE = 0x0A    # Register for Blue data
REG_CLEAR = 0x0B   # Register for Clear data

# Function to write to a register using `writeto_mem`
def write_register(reg, value):
    try:
        i2c.writeto_mem(VEML6040_ADDR, reg, bytearray([value]))
    except Exception as e:
        print(f"Failed to write to register {reg}: {e}")

# Function to read 16-bit data from a register using `readfrom_mem`
def read_register(reg):
    try:
        data = i2c.readfrom_mem(VEML6040_ADDR, reg, 2)  # Read 2 bytes (16-bit data)
        return int.from_bytes(data, 'little')  # Convert to integer (little-endian)
    except Exception as e:
        print(f"Failed to read from register {reg}: {e}")
        return 0

# Function to initialize the VEML6040 sensor
def init_sensor():
    try:
        # Example configuration: Enable the sensor with default settings
        config_value = 0x00  # Adjust based on the VEML6040 datasheet
        write_register(REG_CONFIG, config_value)
        print("Sensor initialized successfully.")
        time.sleep(0.1)  # Allow time for the sensor to initialize
    except Exception as e:
        print(f"Failed to initialize sensor: {e}")

# Function to map raw values to the normalized RGB output
def map_to_rgb(raw_value):
    # Example simple mapping: Adjust this based on your data
    if raw_value < 1000:
        return int((raw_value / 1000) * 255)  # Scale to 0-255
    elif raw_value < 3000:
        return int(((raw_value - 1000) / 2000) * 255)  # Scale to 255 range
    else:
        return 255  # Maximum value for higher raw values

# Function to read RGB data and output as text
def read_rgb():
    try:
        red = read_register(REG_RED)
        green = read_register(REG_GREEN)
        blue = read_register(REG_BLUE)
        clear = read_register(REG_CLEAR)

        # Apply the mapping for each color
        red_normalized = map_to_rgb(red)
        green_normalized = map_to_rgb(green)
        blue_normalized = map_to_rgb(blue)

        print(f"Raw Values: Red={red}, Green={green}, Blue={blue}, Clear={clear}")
        print(f"Normalized RGB: R={red_normalized}, G={green_normalized}, B={blue_normalized}")
    except Exception as e:
        print(f"Error reading RGB data: {e}")

# Initialize the sensor
init_sensor()

# Main loop to continuously read RGB values
while True:
    read_rgb()
    time.sleep(3)  # Read values every 3 seconds